/**
 * @file rubber_wheel_cal.c
 * @author lwz (3296626485@qq.com)
 * @version 2025 BNGU
 * @date 2025-05-13
 */

#include "main.h"
#include "math.h"
#include "rubber_wheel_cal.h"

 /*
 *********************************************************************************************************
 *                                            	变量定义
 *********************************************************************************************************
 */
const float Wheel_R;
const float Robot_R=1;
#define SQUARE(x) ((x)* (x))
Wheel_Data Small_cake;

/*
*********************************************************************************************************
*                                               函数实现
*********************************************************************************************************
*/
double calculate_angle(double vx, double vy) 
{
    double angle_degrees = atan2(vx, vy) * (180.0 / PI);
    if (angle_degrees < 0) 
    {
        angle_degrees += 360.0;
    }
    return angle_degrees;
}

void rubber_wheel_init(Wheel_Data* wheel,float max_speed)
{
    wheel->Robot_Speed_max = max_speed;
}

//在世界坐标系下，速度分量 Vx和Vy分别表示机器人在全局坐标系的 x 轴和 y 轴方向上的速度。
//而在自身坐标系下，速度分量通常表示为机器人前进方向（通常是 x 轴）和横向方向（通常是 y 轴）的速度。

/**
 * @brief 机器人速度分解,舵轮位置计算
 */
void robot_velocity_to_rubber_position_to_wheel_speed(Wheel_Data* robot)
{
    float Vx, Vy, Vw;
    Vy = -robot->Robot_in_self.velocity_exp.Vx;
    Vx = robot->Robot_in_self.velocity_exp.Vy;
    Vw = robot->Robot_in_self.velocity_exp.Vw;

    robot->Wheel_Angle[0] = (calculate_angle(Vy + Vw * (sqrtf(2) / 2) * Robot_R, Vx - Vw * (sqrtf(2) / 2) * Robot_R)) / 360 * 8191;
    robot->Wheel_Angle[1] = (calculate_angle(Vy - Vw * (sqrtf(2) / 2) * Robot_R, Vx - Vw * (sqrtf(2) / 2) * Robot_R)) / 360 * 8191;
    robot->Wheel_Angle[2] = (calculate_angle(Vy - Vw * (sqrtf(2) / 2) * Robot_R, Vx + Vw * (sqrtf(2) / 2) * Robot_R)) / 360 * 8191;
    robot->Wheel_Angle[3] = (calculate_angle(Vy + Vw * (sqrtf(2) / 2) * Robot_R, Vx + Vw * (sqrtf(2) / 2) * Robot_R)) / 360 * 8191;
    robot.Wheel_Speed[0] = sqrtf(SQUARE(Vx + Vw * (sqrtf(2) / 2) * Robot_R) + SQUARE(Vy - Vw * (sqrtf(2) / 2) * Robot_R));
    robot.Wheel_Speed[1] = sqrtf(SQUARE(Vx - Vw * (sqrtf(2) / 2) * Robot_R) + SQUARE(Vy - Vw * (sqrtf(2) / 2) * Robot_R));
    robot.Wheel_Speed[2] = sqrtf(SQUARE(Vx - Vw * (sqrtf(2) / 2) * Robot_R) + SQUARE(Vy + Vw * (sqrtf(2) / 2) * Robot_R));
    robot.Wheel_Speed[3] = sqrtf(SQUARE(Vx + Vw * (sqrtf(2) / 2) * Robot_R) + SQUARE(Vy + Vw * (sqrtf(2) / 2) * Robot_R));
}

//机器人世界坐标系转自身坐标系速度
void World_To_Self(Wheel_Data* robot)
{
    float Vx, Vy, YAW;
    YAW = robot->Robot_in_world.position_exp.yaw * PI / 180;
    Vx = robot->Robot_in_world.velocity_exp.Vx;
    Vy = robot->Robot_in_world.velocity_exp.Vy;
    robot->Robot_in_self.velocity_exp.Vx = Vx * cosf(YAW) - Vy * sinf(YAW);
    robot->Robot_in_self.velocity_exp.Vy = Vx * sinf(YAW) + Vy * cosf(YAW);
	robot->Robot_in_self.velocity_exp.Vw = robot->Robot_in_world.velocity_exp.Vw;
}

void Wheel_Data_To_Motor(Wheel_Data* robot,DJI_Motor_ControlTypeDef* rubber_motor, DJI_Motor_ControlTypeDef* wheel_motor)
{
    for (int i = 0; i < 4; i++)
    {
        rubber_motor->M3508[i].exp_position = (int16_t)(robot->Wheel_Angle[i]);
        wheel_motor->M3508[i].exp_speed = (int16_t)(robot->Wheel_Speed[i]);
    }
}
